#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>	
#include <sys/time.h>
#include "matlite.h"
#include "matlite.cpp"


using namespace std;
using namespace PlayerCc;

int main(int argc, char** argv){
double x,y,t;
matlite ROBOTS;
int N_ROBOT=6;

// Robots 
//[0] x cord
//[1] y cord
//[2] t cord
//[3] port
//[4]

//PlayerClient ListaRobots[6];
//playerc_device_info_t ListaRobots[6];


//vector<PlayerClient(std::basic_string<char>,int)> lista;
//vector<PlayerClient> lista;

ROBOTS.resize(4,N_ROBOT);


PlayerClient robot_0(gHostname, 6665);

//lista[0]=robot_0(gHostname, 6665);
//vector<PlayerClient> lista(gHostname, 6666);
PlayerClient robot_1(gHostname, 6666);
PlayerClient robot_2(gHostname, 6667);
PlayerClient robot_3(gHostname, 6668);
PlayerClient robot_4(gHostname, 6669);
PlayerClient robot_5(gHostname, 6670);
Position2dProxy pp_0(&robot_0, gIndex);	//This is needed in every player client application
Position2dProxy pp_1(&robot_1, gIndex);	//This is needed in every player client application
Position2dProxy pp_2(&robot_2, gIndex);	//This is needed in every player client application
Position2dProxy pp_3(&robot_3, gIndex);	//This is needed in every player client application
Position2dProxy pp_4(&robot_4, gIndex);	//This is needed in every player client application
Position2dProxy pp_5(&robot_5, gIndex);	//This is needed in every player client application



while (true){


//for (int i=0;i<N_ROBOT;i++){
//PlayerClient robot(gHostname, 6665+i);
//Position2dProxy pp(&robot, gIndex);	//This is needed in every player client application
robot_0.Read();
robot_1.Read();
robot_2.Read();
robot_3.Read();
robot_4.Read();
robot_5.Read();

x=pp_0.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_0.GetYPos();
t=pp_0.GetYaw();

ROBOTS.val[0][0]=x;
ROBOTS.val[1][0]=y;
ROBOTS.val[2][0]=t;
ROBOTS.val[3][0]=robot_0.GetPort();

x=pp_1.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_1.GetYPos();
t=pp_1.GetYaw();

ROBOTS.val[0][1]=x;
ROBOTS.val[1][1]=y;
ROBOTS.val[2][1]=t;
ROBOTS.val[3][1]=robot_1.GetPort();

x=pp_2.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_2.GetYPos();
t=pp_2.GetYaw();

ROBOTS.val[0][2]=x;
ROBOTS.val[1][2]=y;
ROBOTS.val[2][2]=t;
ROBOTS.val[3][2]=robot_2.GetPort();

x=pp_3.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_3.GetYPos();
t=pp_3.GetYaw();

ROBOTS.val[0][3]=x;
ROBOTS.val[1][3]=y;
ROBOTS.val[2][3]=t;
ROBOTS.val[3][3]=robot_3.GetPort();

x=pp_4.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_4.GetYPos();
t=pp_4.GetYaw();

ROBOTS.val[0][4]=x;
ROBOTS.val[1][4]=y;
ROBOTS.val[2][4]=t;
ROBOTS.val[3][4]=robot_4.GetPort();

x=pp_5.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp_5.GetYPos();
t=pp_5.GetYaw();

ROBOTS.val[0][5]=x;
ROBOTS.val[1][5]=y;
ROBOTS.val[2][5]=t;
ROBOTS.val[3][5]=robot_5.GetPort();




//}
 






printf("Matrice ROBOTS:\n");
for (int y=0; y<N_ROBOT; y++){
printf("[%f %f %f %f] \n",ROBOTS.val[0][y],ROBOTS.val[1][y],ROBOTS.val[2][y],ROBOTS.val[3][y]);


}

}


//PlayerClient robot(gHostname, 6669);	//This is needed in every player client application
//Position2dProxy pp(&ListaRobots[1], gIndex);	//This is needed in every player client application


//ListaRobots[1].Read();				//Obtain DR and laser data
//x=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
//y=pp.GetYPos();



//printf("il robot di index %i è alla posizione [%f, %f]",ListaRobots[1].GetPort(),x,y);

//printf("device info: driver name: %s ",&rinfo.front().drivername(*)[1]);



}
